//
// gogo35.c - Contains the core functions of the gogo board firmware
//
// Copyright (C) 2001-2007 Massachusetts Institute of Technology
// Contact   Arnan (Roger) Sipiatkiat [arnans@gmail.com]
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation version 2.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//

#case


//#include <18F458.H>
#include <16F877A.H>


// Note: Don't put the option ICD=True here. It disables the Serial interrupt!
#device ADC=10 *=16


#fuses HS,NOWDT,NOPROTECT, BROWNOUT, NOLVP, PUT

// HS - we're using an external high frequency crystal
// NOPROTECT (No Code Protect) - with code protect the memory fuse will be
//          burnt out preventing the chip from being reprogrammed.
// BROWNOUT (Brownout protection) - Brownout is when the supply voltage
//          temporarily drops below the PIC operating voltage. The PIC has
//          a brownout protection circuit that holds the chip on reset until
//          there is sufficiant supply voltage.
// NOLVP (No low voltage programming) - LVP is used for In-circuit serial
//          programming.
// PUT ( up timer) - Makes sure there is sufficient supply voltage before
//          starting the PIC






#use DELAY(clock=20000000)



#include <GOGO35.H>

#use rs232(baud=9600,UART1)
#use i2c(master, sda=I2C_SDA, scl=I2C_SCL, FORCE_HW)

#include <logovm.h>
#include <stdlib.H>



#use fast_io(A)
#use fast_io(B)
//#use fast_io(C)  // somehow this doesn't work. Compiler bug?
#use fast_io(D)
#use fast_io(E)






#define defaultPort      0
#define channelSwitchDelay   50   // delay time in us after switching adc channels
                              // Don't decrease this value without testing.
                              // If the delay is too short (i.e. 10us) the adc won't
                              // have enough time to stabilize before reading the
                              // next channel.

/// Timer 1 Counter
/// This value should be 3036 ( 65536 - 0.1/(4/20000000) * 8)
/// where 0.1 is the time in seconds we want the ISR to be called
/// 4/20000000 is the time each counter takes to tick
/// 8 is the timer divider ... see setup_timer_1()

#define T1_COUNTER      3036     // tick rate = 1.60/8 uSec (value according to the math)


#define  CMD_TIMEOUT_PERIOD  2     // determins how long befor the board will reset
                                    // the command state. Units in 1/10 of a second

#if defined(__PCM__)
   // reserve memory for the bootloader. This allows firmware updates via the serial port
   #org 0x1d00, 0x1FFF void loader16F876(void) {}
   
   // This memory area is used to store Cricket Logo commands
   #org 0x1c00, 0x1cff void cricketLogoMemoryArea(void) {}

#elif defined(__PCH__)
   // reserve memory for the bootloader. This allows firmware updates via the serial port
   #org 0x7A00, 0x7FFF void loader16F876(void) {}  // these are byte addresses. 
                                                   // equiv word addresses are 3D00 - 3FFF
   
   // This memory area is used to store Cricket Logo commands
   #org 0x7200, 0x79DF void cricketLogoMemoryArea(void) {} // equiv word addresses are 3900 - 3DEF
#endif



/////////////////////////////////////////////////////////////////
//  Function Declaration
/////////////////////////////////////////////////////////////////
void setHigh(IOPointer Pin);
void setLow(IOPointer Pin);
int  readPin(IOPointer Pin);

short getBit(int InByte, int BitNo);
void setBit(int *InByte, int BitNo);
void clearBit(int *InByte, int BitNo);

void Ping(int Param);
void TalkToMotor(int MotorBits);
void MotorControl(int MotorCmd);
void SetMotorPower(int Power);
void ChangeMotorPower(int delta);
void sortMtrDuty();
void SetMotorMode(int motorMode); // normal or servo

void ENHigh(int groupNo);
void ENLow(int groupNo);

void MotorON(int MotorNo);
void MotorOFF(int MotorNo);
void MotorRD(int MotorNo);
void MotorThisWay(int MotorNo);
void MotorThatWay(int MotorNo);
void MotorCoast(int MotorNo);
void miscControl(int cur_param, int cur_ext, int cur_ext_byte);

void beep();

void SetBurstMode(int SensorBits, int Mode);
void DoSensorStuff();
// this prints the result back to the PC
//#inline int16 outputSensor(int Target, int readMode);
// this one just returns the sensor value
unsigned int16 readSensor(int sensorNo);
long getSensorVal();
void switchAdcChannel(int channelNo);

void ProcessInput();
void ProcessRFInput();
//int  process_input();
//void Add_to_CMD(int InByte);
//void EndCMD();
//byte get_cmd_buff(int *cmd_ptr);
void init_variables();
void intro ();
void Halt();
void initBoard();

timer2ISR();

/////////////////////////////////////////////////////////////////
//  Global Variables
/////////////////////////////////////////////////////////////////

IOPointer  MotorENPins [MotorCount]={  MTR1_EN, MTR2_EN, MTR3_EN, MTR4_EN};
IOPointer  MotorCWPins [MotorCount]={  MTR1_CW, MTR2_CW, MTR3_CW, MTR4_CW};
IOPointer  MotorCCPins [MotorCount]={  MTR1_CC, MTR2_CC, MTR3_CC, MTR4_CC};

unsigned int CMD_STATE;

int gbl_cur_cmd, gbl_cur_param, gbl_cur_ext, gbl_cur_ext_byte;
int gblBurstModeBits;
int gblBurstModeCounter=0;   // tracks which sensor is the current burst mode sensor

int1 gblSlowBurstMode=0;  // determinds which burst mode we're in (0=normal, 1=slow)
int1 gblSlowBurstModeTimerHasTicked=0;  // ticks every 1/72 sec (by timer0)


int gblCurSensorChannel;

int gblMotorMode=0b00000000;   // default to normal mode
int gblActiveMotors;
int gblMotorDir=0;
int gblMotorONOFF = 0;
int gblMtrDuty[MotorCount+1] = {0xff,0xff,0xff,0xff,0xff};  // Motor PWM Duty cycle
//int gblMtrDutyIndex[MotorCount+1] = {0,1,2,3,4}; // index to the sorted gblMtrDuty[]
unsigned int gblTimer0Counter = MotorCount; // Motor duty cycle counter.
unsigned int gblDutyCycleFlag = 0; // used to find the next duty cycle in timer0
unsigned int gblCurrentDutyIndex = 0; // keeps track of the current duty cycle being used.

// These two variables are for the NEWIR, IR commands in Cricket Logo
// We replace the IR with the serial comm, of course.
unsigned char gblMostRecentlyReceivedByte;
int1 gblNewByteHasArrivedFlag = 0;


int1 gblLogoIsRunning = 0;     // flags if logo procedures are runing
int1 gblButtonPressed = 0;    // flags when the run button is pressed
int1 gblBtn1AlreadyPressed = 0;
unsigned int16 gblWaitCounter =0;  // used for the wait cmd in Logo vm


unsigned int16 gblTimer = 0;   // This is the timer for the TIMER and RESETT commands

int gblCmdTimeOut = 0; // counter to make sure the command state is not stuck somewhere

int gblSerialBuffer[SERIAL_BUFFER_SIZE];
int gblSerialBufferPutIndex=0;
int gblSerialBufferGetIndex=0;
int gblSerialBufferIsFull=FALSE;

#if defined(__PCH__)
char gblFlashBuffer[getenv("FLASH_ERASE_SIZE")]; // buffer for flash write operations
char gblFlashBufferPtr=0; // pointer with-in the flash buffer
int16 gblFlashBaseAddress; // where the flash buffer shuld be written to in the flash mem
#endif

/////////////////////////////////////////////////////////////////////////
//
//   I N T E R R U P T    S E R V I C E    R O U T I N E S
//
/////////////////////////////////////////////////////////////////////////



#int_rda
serial_isr(void)
{

   if (gblSerialBufferIsFull == FALSE)
   {
      gblSerialBuffer[gblSerialBufferPutIndex] = getchar();         
      gblSerialBufferPutIndex++;
      if (gblSerialBufferPutIndex >= SERIAL_BUFFER_SIZE)
         gblSerialBufferPutIndex = 0;   
      
      if (gblSerialBufferPutIndex == gblSerialBufferGetIndex)
         gblSerialBufferIsFull = TRUE;
     
   }
}



#int_rtcc                          // This function is called every time
clock_isr() {                      // the RTCC (timer0) post scale overflows (255->0).
                                   // we have set the count rate to be
                                   // 20000000/4 MHz / 256.



int i;

unsigned int minDuty;
unsigned int nextDutyIndex;
unsigned int periodTilNextInterrupt;




do {


   ////////////////////////////////////////////////////////////////////////////
   //
   // perform the motor operations
   //
   //

   if (gblTimer0Counter < MotorCount) {
      if (getBit(gblMotorONOFF, gblCurrentDutyIndex) == ON) {
         if (gblMtrDuty[gblCurrentDutyIndex] < 255) {
            if (getBit(gblMotorMode, gblCurrentDutyIndex) == MOTOR_NORMAL) {
               if (getBit(gblMotorDir, gblCurrentDutyIndex))
                  setLow(MotorCWPins[gblCurrentDutyIndex]);
               else
                  setLow(MotorCCPins[gblCurrentDutyIndex]);
            }
            else
               setLow(MotorCCPins[gblCurrentDutyIndex]);
         }
      }
   }
   else {
      for (i=0 ; i<MotorCount ; i++)  {
         if (getBit(gblMotorONOFF, i) == ON) {
            if (gblMtrDuty[i] > 0) {
               if (getBit(gblMotorMode, i) == MOTOR_NORMAL) {
                  if (getBit(gblMotorDir, i))
                     setHigh(MotorCWPins[i]);
                  else
                     setHigh(MotorCCPins[i]);
               }
               else
                  setHigh(MotorCCPins[i]);
            }
         }
      }
   }


   ///////////////////////////////////////////////////////////////////////////
   //
   //  Determine the time (counts) until the next interrupt
   //
   //

         // find the next duty cycle
         // - this is the one with the smallest value
         // - in a cycle (MotorCount+1 times) we won't use the same value
         //   so, we check gblDutyCycleFlag to prevent this.

         minDuty = 255;

         for (i=0;i<=MotorCount;i++) {
            if ( (minDuty >= gblMtrDuty[i]) && !(getBit(gblDutyCycleFlag,i)) ) {
               minDuty = gblMtrDuty[i];
               nextDutyIndex = i;
            }
         }

         setBit(&gblDutyCycleFlag, nextDutyIndex); // make the for loop skip this duty cycle

         // determin the time (counts) till the next interrupt.
         // - This is the difference between the next and the current duty
         //   unless we reach the last duty. The smallest duty is used in this case
         if (gblTimer0Counter < MotorCount)
            periodTilNextInterrupt = minDuty - gblMtrDuty[gblCurrentDutyIndex];
         else
            periodTilNextInterrupt = minDuty;

         gblCurrentDutyIndex = nextDutyIndex;

         // by the time we get to the MotorCount-1 count, all the flags would
         // have been set. So, clear them.
         if (gblTimer0Counter == MotorCount-1) gblDutyCycleFlag = 0;

         // advance timer0 count
         if (gblTimer0Counter < MotorCount)
            gblTimer0Counter++;
         else
            gblTimer0Counter = 0;

   } while ((periodTilNextInterrupt == 0) && (gblTimer0Counter > 0));




// set the duration for the next interrupt
set_rtcc(255-periodTilNextInterrupt);

// trigger gblSlowBurstModeTimerHasTicked every cycle (~76Hz).
if (gblTimer0Counter == 0) {
   gblSlowBurstModeTimerHasTicked=1;  // this triggers the slow burst mode to send data. See DoSensorStuff()
}


}


// timer1 is interrupting every 100ms.
// see setup_timer_1() in and set_timer1 ()

#int_timer1
timer1ISR() {

      gblTimer++;  // increase the global timer

      // if the board is receiving a command from a computer
      // then increase the timeout counter
      if (CMD_STATE != WAITING_FOR_FIRST_HEADER) {
         gblCmdTimeOut++;  // Command timout counter. Used at the end of main()
      }

      // the Wait opcode sets gblWaitCounter,
      // then waits until it gets to 0.
      if (gblWaitCounter > 0) { gblWaitCounter--; }


      ////////////////////////////////////////////
      // check menu button (button 1)
      // if it is pressed
      if (input(RUN_BUTTON)) {
            // if button not already pressed
            if ( !gblBtn1AlreadyPressed) {
               gblButtonPressed = !gblButtonPressed;
               gblBtn1AlreadyPressed=1;
               gblWaitCounter = 0;  // reset wait command (so the running Logo wait code
                                    // stops immediately).
               gblONFORNeedsToFinish = 0; // incase an onfor is running.


              ////////////////////////////////////////////////////////
              // run Logo procedures
              if (!gblLogoIsRunning)
              {
                  srand(gblTimer);  // seed for the random function;
                  output_high(RUN_LED);
                  /////////////////////////////////////////////////
                  //   get the address of the procedure
                  //   to run.

                  // gblMemPtr is defined in eeprom.h
//                  gblMemPtr = read_program_eeprom(RUN_BUTTON_BASE_ADDRESS) << 8;
//                  gblMemPtr += read_program_eeprom(RUN_BUTTON_BASE_ADDRESS+1);

                 #if defined(__PCM__)
                  gblMemPtr = read_program_eeprom(RUN_BUTTON_BASE_ADDRESS+1);
                 #elif defined (__PCH__)
                  gblMemPtr = read_program_eeprom( (RUN_BUTTON_BASE_ADDRESS+1)<<1 );
                 #endif

                  clearStack();
                  gblNewByteHasArrivedFlag=0;
                  gblLogoIsRunning = 1;
               } else {  // stop Logo
                  gblLogoIsRunning = 0;
                  output_low(RUN_LED);
               }
              ////////////////////////////////////////////////////////

            }
      } else if (gblBtn1AlreadyPressed) {
         gblBtn1AlreadyPressed=0;
      }

      set_timer1(T1_COUNTER);  // this is to keep the interrupt period constant (100ms)



//portc2 = !portc2;
//RUN_LED_ON;
}

// timer2 is neccessary for the PIC's hardware PWM (which drives the beeper)
#int_timer2
timer2ISR() {

}


//////////////////////////////////////////////////
//   In the following few functions ..
//
//    (Pin>>3) is the port number (5=PortA, 6=B, ...)
//    (Pin&7) is the pin number  (0-7)
//
//    So,
//
//    *(Pin>>3) is a pointer to the port
//    1<<(Pin&7) is a bit mask used to set/get a bit
//
//    Consult the PIC-C FAQ for more info.

void setLow(IOPointer Pin)
{
   *(Pin>>3) &= (~(1<<(Pin&7)));
}

void setHigh(IOPointer Pin)
{
   *(Pin>>3) |= (1<<(Pin&7));

}


int readPin(IOPointer Pin)
{
   return (*(Pin>>3) & (1<<(Pin&7))) ;

}



//////////////////////////////////////////////////
short getBit(int InByte, int BitNo)
{  return ((InByte >> BitNo) & 1);
}

void setBit(int *InByte, int BitNo)
{  *InByte |= (1<<BitNo);
}

void clearBit(int *InByte, int BitNo)
{  *InByte &= ~(1<<BitNo);
}



/////////////////////////////////////////////////////////////////////////
//
//   M O T O R   C O N T R O L
//
/////////////////////////////////////////////////////////////////////////

void TalkToMotor(int MotorBits)
{

   // Each bit represents one motor. i.e 0b00010010 means motor 2 and 5 are active
   gblActiveMotors = MotorBits;

//   printf("%c%c%c", ReplyHeader1, ReplyHeader2, ACK_BYTE);  // send acknowledge byte

}


void MotorControl(int MotorCmd)
{
   int i;

   for (i=0;i<MotorCount;i++)
   {
      if ((gblActiveMotors >> i) & 1 )
      {
         SetMotorMode(MOTOR_NORMAL);

         switch (MotorCmd)
         {
         case MTR_ON:   MotorON(i);
               break;
         case MTR_OFF: MotorOFF(i);
               break;
         case MTR_RD: MotorRD(i);
               break;
         case MTR_THISWAY: MotorThisWay(i);
               break;
         case MTR_THATWAY: MotorThatWay(i);
               break;
         case MTR_COAST: MotorCoast(i);
               break;
         }

      }
   }


}

/////////////////////////////////////////////////
// Refer to the problem explained in MotorControl() if
// this function suddenly missbehave
void SetMotorPower(int Power)
{
   int i;

   for (i=0;i<MotorCount;i++)
   {
      if ((gblActiveMotors >> i) & 1 )
      {

         // Starting from GoGo2.2ms5 and higher, a better implementation of
         // motor PWM has been implemented (see timer0). Now, the user can
         // control up to 255 power levels (instead of 8). But to keep the
         // library backward compatible, the first 8 power levels are mapped
         // out evenly between 0-255.

         switch (Power) {
//            case 0: Power = 0x00; break;  // commented out because values don't change anyway
            case 1: Power = 36; break;
            case 2: Power = 73; break;
            case 3: Power = 110; break;
            case 4: Power = 146; break;
            case 5: Power = 183; break;
            case 6: Power = 219; break;
            case 7: Power = 255; break;
         }

         gblMtrDuty[i] = Power;

      }
   }

}


void ChangeMotorPower(int delta) {
   int i;

   for (i=0;i<MotorCount;i++)
   {
      if ((gblActiveMotors >> i) & 1) {
         gblMtrDuty[i] = gblMtrDuty[i] + delta;
      }
   }
//   sortMtrDuty();

}


// Set the mode of the active motors (NORMAL or SERVO)
void SetMotorMode(int motorMode) {
   int i;

   for (i=0;i<MotorCount;i++) {
      if ((gblActiveMotors >> i) & 1)
         if (motorMode == MOTOR_NORMAL)
            clearBit(&gblMotorMode, i);
         else // Servo mode
            setBit(&gblMotorMode, i);

   }
}


// Sets "both" the EN pins of a h-bridge chip
void ENHigh(int groupNo) {
   groupNo<<=1;

   setHigh(MotorENPins[groupNo]);
   setHigh(MotorENPins[groupNo+1]);

   // power on the motor chip
   if (!groupNo) {
      output_high(MOTOR_AB_EN);
   } else {
      output_high(MOTOR_CD_EN);
   }

}


// Clears "both" the EN pins of a h-bridge chip
// but do it only if both motors on the chip
// is in the coast state
void ENLow(int groupNo) {
   groupNo<<=1;

   setLow(MotorENPins[groupNo]);
   setLow(MotorENPins[groupNo+1]);

   // power off the motor chip
   if (!groupNo) {
      output_low(MOTOR_AB_EN);
   } else {
      output_low(MOTOR_CD_EN);
   }


}



void MotorON(int MotorNo)
{
   IOPointer MtrCC, MtrCW;

   MtrCW       = MotorCWPins[MotorNo];
   MtrCC       = MotorCCPins[MotorNo];

   if (getBit(gblMotorDir,MotorNo))
        {    setLow(MtrCC); setHigh(MtrCW);   }
   else
      {   setHigh(MtrCC); setLow(MtrCW);   }

   // In gogo board 2.3, we have to set both the EN pins on the h-bridge
   // to turn on one motor port. That is
   //  Motor 1 needs EN1 and EN2 to be high
   //  Motor 2 needs the same as motor 1
   //  Motor 3 needs EN3 and EN4 to be high
   //  Motor 4 needs the same as motor 3
   //
   //  ENHigh handles this.
   ENHigh(MotorNo>>1);

   setBit(&gblMotorONOFF,MotorNo);
}



void MotorOFF(int MotorNo)
{
   IOPointer MtrCC, MtrCW;

   MtrCW       = MotorCWPins[MotorNo];
   MtrCC       = MotorCCPins[MotorNo];

   setHigh(MtrCC);
   setHigh(MtrCW);

   clearBit(&gblMotorONOFF,MotorNo);
   ENHigh(MotorNo>>1);     // this ensures the motor breaks. Sometimes the timer_isr turns off the enable pin, which turns 'stop' into 'coast'

}



void MotorRD(int MotorNo)
{   IOPointer MtrCC, MtrCW;

    MtrCW       = MotorCWPins[MotorNo];
    MtrCC       = MotorCCPins[MotorNo];

      if (getBit(gblMotorDir,MotorNo))
      {   if (getBit(gblMotorONOFF, MotorNo)) {
            setLow(MtrCW);
            setHigh(MtrCC);
         }
         clearBit(&gblMotorDir,MotorNo);
      } else
      {   if (getBit(gblMotorONOFF, MotorNo)) {
            setHigh(MtrCW);
            setLow(MtrCC);
         }
         setBit(&gblMotorDir,MotorNo);
      }

}


///////////////////////////////////////////////////////////////////


void MotorThisWay(int MotorNo)
{   IOPointer MtrCC, MtrCW;

     MtrCW       = MotorCWPins[MotorNo];
     MtrCC       = MotorCCPins[MotorNo];

     setBit(&gblMotorDir,MotorNo);

      if (getBit(gblMotorONOFF, MotorNo)) {
          setLow(MtrCC);
            setHigh(MtrCW);
      }

}


void MotorThatWay(int MotorNo)
{   IOPointer MtrCC, MtrCW;

     MtrCW       = MotorCWPins[MotorNo];
     MtrCC       = MotorCCPins[MotorNo];

     clearBit(&gblMotorDir,MotorNo);

      if (getBit(gblMotorONOFF, MotorNo)) {
         setLow(MtrCW);
           setHigh(MtrCC);
      }

}


void MotorCoast(int MotorNo)
{
   IOPointer MtrCC, MtrCW;

     MtrCW       = MotorCWPins[MotorNo];
     MtrCC       = MotorCCPins[MotorNo];

        clearBit(&gblMotorONOFF,MotorNo);

      setLow(MtrCW);
      setLow(MtrCC);

   // if both ports on the h-bridge is off then turn off the
   // EN pins
   if (  !((gblMotorONOFF >> (MotorNo & 0b10)) & 1) &&
         !((gblMotorONOFF >> ((MotorNo & 0b10) + 1)) & 1) ) {
            ENLow(MotorNo>>1);
         }
}

///////////////////////////////////////////////////////////
//
//    M i s c   C o n t r o l
//
///////////////////////////////////////////////////////////

void miscControl(int cur_param, int cur_ext, int cur_ext_byte)
{
//   int16 counter;

   switch (cur_param) {
      case MISC_USER_LED:
         if (cur_ext == TURN_USER_LED_ON) { USER_LED_ON; }
         else {USER_LED_OFF; }
         break;
      case MISC_BEEP:
         beep();
         break;
      case MISC_SET_PWM:
         MotorControl(MTR_ON);
         MotorControl(MTR_THISWAY);
         SetMotorMode(MOTOR_SERVO);
         SetMotorPower(cur_ext_byte);
         break;

// we handle EEPROM upload in the main loop (urr .. ugly code)
      case MISC_UPLOAD_EEPROM:

         break;

      case MISC_I2C_SETUP:
         switch (cur_ext) {
            case I2C_START:
               i2c_start();
               break;
            case I2C_STOP:
               i2c_stop();
               break;
            case I2C_WRITE:
               i2c_write(cur_ext_byte);
//               printf("%c", cur_ext_byte);
               break;
            case I2C_READ:
               i2c_read(0);
               break;
            }
         break;


   }
}


/////////////////////////
//    User LED controls

void uLED_on()
{  output_high(USER_LED);
}

void uLED_off()
{  output_low(USER_LED);
}


///////////////////////
//   Beep

void beep() {
    set_pwm1_duty(50);        // make a beep
    delay_ms(50);
    set_pwm1_duty(0);        // make a beep
}




/////////////////////////////////////////////////////////////////////////
//
//   P R O C E S S   S E N S O R    W H I L E    I D L E
//
/////////////////////////////////////////////////////////////////////////



void DoSensorStuff()
{
//   int i;
   long SensorVal;


//      for (i=0;i<8;i++)
//      {


            //////////////////////////////////////////////////////////////////
            //
            //  Burst sensor data to computer

            if (!gblSlowBurstMode || gblSlowBurstModeTimerHasTicked) {


               // if this sensor is in Burst mode -> send value to computer
               if ((gblBurstModeBits>>gblBurstModeCounter) & 1)
               {
               SensorVal=readSensor(gblBurstModeCounter);
                   printf("%c%c%c", 0x0c, (gblBurstModeCounter << 5) | (SensorVal >> 8), SensorVal & 0xff);
               }

               // gblBurstModeCounter will circulate from 0 - 7
               gblBurstModeCounter = (gblBurstModeCounter+1) % 8;
               gblSlowBurstModeTimerHasTicked = 0;
             }




}


/////////////////////////////////////////////////////////////////////////
//
//   S E N S O R   C O N T R O L
//
/////////////////////////////////////////////////////////////////////////


void SetBurstMode(int SensorBits, int Mode)
{

   gblBurstModeBits = SensorBits;
   if (Mode > 0)
      gblSlowBurstMode = 1; // switch to SLOW burst mode.
   else
      gblSlowBurstMode = 0; // switch to normal (fast) burst mode.


//   printf("%c%c%c", ReplyHeader1, ReplyHeader2, ACK_BYTE);

}


unsigned int16 readSensor(int sensorNo) {
      if (gblCurSensorChannel != sensorNo)
      {   switchAdcChannel(sensorNo);
           gblCurSensorChannel=sensorNo;
      }
      return(getSensorVal());
}



/////////////////////////////////////////////////
long getSensorVal()
{
   long sensorVal;

   sensorVal=read_adc();
   //return (sensorVal >> 6);  // use this line if using PIC-C compiler version 2.x

   // the PIC 16F77 ADC is only 8 bits. So, we simulate 10 bits by shifting
   // left 2 bits. But why in the code we shif right 6 bits? This is because
   // somehow the 8 bit sensor readings are stored in the higher byte
   // of the long variable. So, we have to shift right 8 bits before shifting
   // left. So, 8-2 is 6.
   #IFDEF PIC16F77
   sensorVal >>= 6;
   #ENDIF

   return (sensorVal);        // this line works with PIC-C compiler version 3.x

}

/////////////////////////////////////////////////
void switchAdcChannel(int channelNo) {
   set_adc_channel(channelNo);
   delay_us(channelSwitchDelay);   // wait for adc to stabilize
}


//////////////////////////////////////////////////////////////////////////////
// 
// Fetch a character from the serial buffer
//
//////////////////////////////////////////////////////////////////////////////

byte readSerialBuffer(byte *charPtr)
{
   int errorCode;
   
   if (gblSerialBufferIsFull == TRUE)
   {
      gblSerialBufferIsFull = FALSE;
      errorCode = SERIAL_OVERFLOW;   
      gblSerialBufferPutIndex = 0;
      gblSerialBufferGetIndex = 0;
      *charPtr = 0;      
   }
   else if (gblSerialBufferGetIndex == gblSerialBufferPutIndex)   
   {
       errorCode = SERIAL_NO_DATA;      
       *charPtr = 0;
   }
   else
   {
      *charPtr = gblSerialBuffer[gblSerialBufferGetIndex];
      gblSerialBufferGetIndex++;
      if (gblSerialBufferGetIndex >= SERIAL_BUFFER_SIZE)
      {  gblSerialBufferGetIndex = 0;
      }
      errorCode = SERIAL_SUCCESS;   
   }
   return(errorCode);
}



/////////////////////////////////////////////////////////////////////////
//
//   M I S C
//
/////////////////////////////////////////////////////////////////////////

void init_variables()
{  
   gblBurstModeBits = 0;

   CMD_STATE = WAITING_FOR_FIRST_HEADER;


   ///////////////////////////////////
   // Logo VM variables
   gblLogoIsRunning=0;         // if set, Lovo VM will start fetching opcode
                     // from EEPROM and run them.
   gblStkPtr=0;      // Stack pointer
   gblInputStkPtr=0; // Procedure input stack pointer
   gblErrFlag=0;     // Error flag. Not really used.



   // init the record pointer to the one last saved in the EEPROM
   // see RECORD in evalopcode.c to see how this value is logged
   #if defined(__PCM__)
   gblRecordPtr = read_ext_eeprom(MEM_PTR_LOG_BASE_ADDRESS) + (read_ext_eeprom(MEM_PTR_LOG_BASE_ADDRESS+1) << 8);
   #elif defined(__PCH__)
   gblRecordPtr = read_program_eeprom(MEM_PTR_LOG_BASE_ADDRESS<<1);
   
   #endif
}


void intro ()
{

    set_pwm1_duty(50);        // make a beep
    USER_LED_ON; RUN_LED_ON;

    delay_ms(50);

    set_pwm1_duty(0);         // stop the beep

    delay_ms(50);
    USER_LED_OFF; RUN_LED_OFF;

    set_pwm1_duty(50);        // make a beep
    delay_ms(50);
    set_pwm1_duty(0);         // stop the beep
    delay_ms(0);

    USER_LED_ON; RUN_LED_ON;
    delay_ms(100);
    USER_LED_OFF; RUN_LED_OFF;


}





void Halt()
{       while (1) {
          output_high(RUN_LED);
          delay_ms(50);
          output_low(RUN_LED);
          delay_ms(500);
       }

}



void initBoard()
{
   int i,j;


   gblActiveMotors = 0;

   set_tris_a(PIC_TRIS_A);
   set_tris_b(PIC_TRIS_B);
   set_tris_c(PIC_TRIS_C);
   set_tris_d(PIC_TRIS_D);
   set_tris_e(PIC_TRIS_E);


   // Init the analog ports
   setup_port_a( ALL_ANALOG );
   setup_adc( ADC_CLOCK_INTERNAL );

   // Init default sensor port
   gblCurSensorChannel=defaultPort;
   set_adc_channel( defaultPort );


   // Disable both motor chips
   output_low(MOTOR_AB_EN);
   output_low(MOTOR_CD_EN);


   // Init all motors to the coast state
   for (i=0,j=0 ; i<MotorCount ; i++)
   {   setLow(MotorENPins[i]);       // Mtr Enable pin
      setLow(MotorCWPins[i]);   // Mtr CW pin.
      setLow(MotorCCPins[i]);      // Mtr CCW pin
   }


   // init the serial port
   //ser_init();



      setup_ccp1(CCP_PWM);   // Configure CCP1 as a PWM (for the beeper)

      setup_timer_1 ( T1_INTERNAL | T1_DIV_BY_8 ); // int every (65536-T1_COUNTER) * 1.6 uSec
                                                    // see T1_COUNTER def for more info
      setup_timer_2(T2_DIV_BY_16,250,16);
        


      enable_interrupts(GLOBAL);
  
      setup_timer_0( RTCC_INTERNAL| RTCC_DIV_256 | RTCC_8_BIT);
      set_rtcc(0);

      enable_interrupts(INT_RTCC);
      enable_interrupts(INT_TIMER1);
      enable_interrupts(INT_TIMER2);
      enable_interrupts(INT_RDA);

      set_timer1(T1_COUNTER);

   /////////////////////////////////////////////////
   //
   //  Logo VM stuff
   //
      init_ext_eeprom();


      intro();


}

#if defined(__PCH__)

///////////////////////////////////////////////////////////////////////////////
//
//   F L A S H   M E M O R Y   R O U T I N E S
//
///////////////////////////////////////////////////////////////////////////////


// setup the flash memory buffer
void flashSetWordAddress(int16 address) {

      // calculate the address of the beginning of the current flash block
      gblFlashBaseAddress = address;
      gblFlashBaseAddress &= ~(int32)((getenv("FLASH_ERASE_SIZE")>>1)-1);
      
      // calculate the position within the current flash block
      gblFlashBufferPtr = address - gblFlashBaseAddress;
      gblFlashBufferPtr <<= 1; // ptr points to each byte in the buffer (not word)
      // read the mem block that will be written to. This makes is easier to write back 
      // because we can just write back the whole block at once.
      read_program_memory(gblFlashBaseAddress<<1, gblFlashBuffer, getenv("FLASH_ERASE_SIZE") );
}

// write to flash (buffered)
void flashBufferedWrite(int16 InByte) {
      
      gblFlashBuffer[gblFlashBufferPtr++] = (int)InByte;
      gblFlashBuffer[gblFlashBufferPtr++] = (int)(InByte>>8);  // each readable flash block is 2 bytes (we use only one here).
                    
      // when flash buffer is full -> write to it
      if (!(gblFlashBufferPtr < (getenv("FLASH_ERASE_SIZE")))) {

         // erase the block
         erase_program_eeprom(gblFlashBaseAddress<<1);
         // write back the block
         write_program_memory(gblFlashBaseAddress<<1, gblFlashBuffer, getenv("FLASH_ERASE_SIZE") );

         gblFlashBufferPtr = 0;
         gblFlashBaseAddress += getenv("FLASH_ERASE_SIZE")>>1;
         read_program_memory(gblFlashBaseAddress<<1, gblFlashBuffer, getenv("FLASH_ERASE_SIZE") );
      }

}

// flash the content of the buffer to the flash
void flashFlushBuffer() {
         // write the remaining bytes to the flash memory
         // erase the block
         erase_program_eeprom(gblFlashBaseAddress<<1);
         // write back the block
         write_program_memory(gblFlashBaseAddress<<1, gblFlashBuffer, getenv("FLASH_ERASE_SIZE") );
}


// write the InByte immediatly to the flash (no buffer)
void flashWrite(int16 InByte) {
      gblFlashBuffer[gblFlashBufferPtr++] = (int)InByte;
      gblFlashBuffer[gblFlashBufferPtr++] = (int)(InByte>>8);  // each readable flash block is 2 bytes (we use only one here).
      
      flashFlushBuffer();

}

#endif

/////////////////////////////////////////////////////////////////////////
//
//   S E R I A L    D A T A    H A N D L E R
//
/////////////////////////////////////////////////////////////////////////


void ProcessInput()
{   byte InByte, buff_status;
   int1 doNotStopRunningProcedure;   // if set means we've got a Logo cmd. We need to echo back.
   
   
   while ((buff_status = readSerialBuffer(&InByte)) == SERIAL_SUCCESS)  // fetch character
   {

            // reset the command timeout counter
            gblCmdTimeOut = 0 ;

            // this is for the IR? command in Cricket Logo
            // No matter what the input bytes are for, we
            // make it available to Cricket Logo.
            gblMostRecentlyReceivedByte = InByte;
            gblNewByteHasArrivedFlag = 1;


            // always send an echo byte back to the PC.
            printf( "%c",InByte);


            switch (CMD_STATE) {


            ///////////////////////////////////////////////////////
            //
            //  if the board is idle, we need to distinguish which
            //  mode we are running, GoGo or Cricket Logo
            case WAITING_FOR_FIRST_HEADER:
                  switch (InByte) {
                    ////////////////////////////////////
                    // this is a GoGo header
                    case InHeader1:
                          CMD_STATE = WAITING_FOR_SECOND_HEADER;
                          doNotStopRunningProcedure = 1;
                          break;
                    ////////////////////////////////////
                    // the rest are Cricket Logo headers
                    case SET_PTR:   // set-pointer
                          CMD_STATE = SET_PTR_HI_BYTE;
                          break;
                    case READ_BYTES:   // read-bytes
                          CMD_STATE = READ_BYTES_COUNT_HI;
                          break;
                    case WRITE_BYTES:   // write-bytes
                          CMD_STATE = WRITE_BYTES_COUNT_HI;
                          break;
                    case RUN:   // run
                          doNotStopRunningProcedure = 1;

                          ///////////////////////////////////
                          // start runing logo
                          output_high(RUN_LED);
                          gblWaitCounter = 0;  // reset wait command (so any running Logo wait code
                                               // stops immediately).
                          gblONFORNeedsToFinish = 0; // in case an onfor is running.
                          clearStack();
                          gblNewByteHasArrivedFlag = 0;
                          gblLogoIsRunning = 1;
                          ////////////////////////////////////

                          break;
                    case CRICKET_CHECK:  // cricket-check
                          CMD_STATE = CRICKET_NAME;
                          break;
                  };

                    // Stop any running logo procedures unless we got a "RUN" command
                    // or if we got a thethered gogo command
                    // which will set doNotStopRunningProcedure to 1
                    if (!doNotStopRunningProcedure) {
                            // stop any running procedures
                            gblLogoIsRunning = 0;
                            gblWaitCounter = 0;
                            output_low(RUN_LED);

                            // stop burst mode
                            gblBurstModeBits = 0;
                    }
                    doNotStopRunningProcedure=0;

                 break;

            /////////////////////////////////////////////////////////////////
            //
            //   These are GoGo states
            //
            case WAITING_FOR_SECOND_HEADER:
                if (InByte == InHeader2) CMD_STATE = WAITING_FOR_CMD_BYTE;
                break;
            case WAITING_FOR_CMD_BYTE:
                 gbl_cur_cmd    =(InByte & 0b11100000) >> 5; // bit 5,6,7 are the command bits
               gbl_cur_param   =(InByte & 0b00011100) >> 2; // bit 2,3,4 are parameter bits for motors and sensors.
               gbl_cur_ext      =(InByte & 0b00000011);

               if (gbl_cur_cmd > ONE_BYTE_CMD)
               {   CMD_STATE = WAITING_FOR_SECOND_CMD_BYTE;
               } else
               {   CMD_STATE = CMD_READY;
               }
               break;
            case WAITING_FOR_SECOND_CMD_BYTE:

                gbl_cur_ext_byte = InByte;
                CMD_STATE = CMD_READY;
                break;
            case CMD_READY:
               break;


            /////////////////////////////////////////////////////////////////
            //
            //   These are Cricket Logo states
            //
            //////////////////////////////////////////////////////////////

            //  set-pointer
            case SET_PTR_HI_BYTE:  /* get addr high byte */
               gblMemPtr = (unsigned int16) InByte << 8;
               CMD_STATE = SET_PTR_LOW_BYTE;
//                     printf("%c",CMD_STATE);
               break;
            case SET_PTR_LOW_BYTE:  /* get addr low byte */

               gblMemPtr = gblMemPtr | InByte;
               CMD_STATE = WAITING_FOR_FIRST_HEADER;


               // Since gogo stores the run button vector at a different memory location,
               // we must map it correctly.
               // so, any thing between 0xff0-0xfff will be mapped to the GoGo's address

               if ((gblMemPtr&0xff0) == 0xff0) {

                  gblMemPtr = (RUN_BUTTON_BASE_ADDRESS + (gblMemPtr&0xf)) - FLASH_USER_PROGRAM_BASE_ADDRESS ;

                  // beep to identify that download is almost done.
                  // Beep is turned off in the next case.
                  set_pwm1_duty(50);

               }
   
               #if defined(__PCH__) 
               
               flashSetWordAddress(FLASH_USER_PROGRAM_BASE_ADDRESS + gblMemPtr);
              
               #endif
               
               
               break;
               

            //////////////////////////////////////////////////////////////
            // read-bytes
            case READ_BYTES_COUNT_HI:  /* get count high byte */
               gblRWCount = (unsigned int16) InByte << 8;
               CMD_STATE = READ_BYTES_COUNT_LOW;
               break;
            case READ_BYTES_COUNT_LOW:  /* get count low byte */
               gblRWCount = gblRWCount | InByte;
               sendBytes(gblMemPtr, gblRWCount);
               gblMemPtr += gblRWCount;  // update the gblMemPtr

               CMD_STATE = WAITING_FOR_FIRST_HEADER;
               break;

            //////////////////////////////////////////////////////////////
            // write-bytes
            case WRITE_BYTES_COUNT_HI:  /* get count high byte */
               gblRWCount = (unsigned int16) InByte << 8;
               CMD_STATE = WRITE_BYTES_COUNT_LOW;
               break;
            case WRITE_BYTES_COUNT_LOW:  /* get count low byte */
               gblRWCount = gblRWCount | InByte;
               CMD_STATE = WRITE_BYTES_SENDING;

            
            
//            #if defined(__PCH__)
//               // if downloading a Logo program -> must erase all the flash memory blocks used to strore the Logo programs 
//               if (gblMemPtr == 0) {
//                  for (memoryBlockAddress=FLASH_USER_PROGRAM_BASE_ADDRESS ; memoryBlockAddress < FLASH_BOOTLOADER_BASE_ADDRESS ; memoryBlockAddress += (getenv("FLASH_ERASE_SIZE")>>1)) {
//                     erase_program_eeprom(memoryBlockAddress<<1 );
//                  }
//               }
//            #endif

               break;
            case WRITE_BYTES_SENDING:
               set_pwm1_duty(0);        // stop any previous beep

            // 14 bit parts (16F877) writes each memory word one by one
            #if defined(__PCM__)
               write_program_eeprom(FLASH_USER_PROGRAM_BASE_ADDRESS + gblMemPtr++, (unsigned int16)InByte);
            
            // 16 bit parts (18F458) buffers the data and writes blocks of data to the falsh
            #elif defined(__PCH__)
               // write to the flash buffer (it automatically writes to the flash
               // every time the buffer is full
               flashBufferedWrite(InByte);
               
            #endif
               

               if (--gblRWCount < 1) { 

                  #if defined(__PCH__)
                     // write remaining data in the buffer to the flash
                     flashFlushBuffer();
                  #endif

                  CMD_STATE = WAITING_FOR_FIRST_HEADER;
               }

               // send back the one's complement value of the received byte
               printf("%c",255-InByte);
               
               break;

            //////////////////////////////////////////////////////////////
            // cricket-check. Assumes we only ping all crickets (InByte = 0)
            // Named crickets are not supported.
            case CRICKET_NAME:
               printf("%c",0x37);
               CMD_STATE = WAITING_FOR_FIRST_HEADER;
               break;



            default:
                CMD_STATE = WAITING_FOR_FIRST_HEADER;
               break;
         }
         if (CMD_STATE == CMD_READY)  break;


   }

   if (buff_status == SERIAL_OVERFLOW)
   {   //Reset
      CMD_STATE = WAITING_FOR_FIRST_HEADER;

   }
   // else assume no data in serial buffer
}



/////////////////////////////////////////////////////////////////////////
//
//   M A I N
//
/////////////////////////////////////////////////////////////////////////


void main() {


int16 SensorVal;
int16 uploadLen,counter;


int16 foo;


init_variables();
initBoard();

while (1) {


   ProcessInput();

   if (CMD_STATE == CMD_READY)
   {

         switch (gbl_cur_cmd)  {
            case CMD_PING:
                  // do nothing, just send back ACK
                  break;

            case CMD_READ_SENSOR:
                  SensorVal = readSensor(gbl_cur_param);
                  break;

            case CMD_MOTOR_CONTROL:
                  MotorControl(gbl_cur_param);
                  break;

            case CMD_MOTOR_POWER:
                  SetMotorPower(gbl_cur_param);
                  break;

            case CMD_TALK_TO_MOTOR:
                  TalkToMotor(gbl_cur_ext_byte);
                  break;

            case CMD_BURST_MODE:
                  SetBurstMode(gbl_cur_ext_byte, gbl_cur_ext);
                  break;

            case CMD_MISC_CONTROL:
                  miscControl(gbl_cur_param, gbl_cur_ext, gbl_cur_ext_byte);
                  break;
            }


      // send the appropriate response back to the computer
      if (gbl_cur_cmd == CMD_READ_SENSOR)
         printf("%c%c%c%c",ReplyHeader1, ReplyHeader2, SensorVal >> 8, SensorVal & 0xff);
      else
         printf("%c%c%c", ReplyHeader1, ReplyHeader2, ACK_BYTE);  // send acknowledge byte


      ///////////////////////////////////////////////////////////
      //
      //  Handle EEPROM uploads here (urrr ... ugly coding)

      /// cur_ext (2 bits) and cur_ext_byte (8 bits) combinds to form a 10 bit counter.
      /// But to cover all the 32K EEPROM address range we must multiply this count by 32 (shift left 5 bits)
      /// Therefore, one unit of the cur_ext|cur_ext_byte represents 32 EEPROM bytes (16 sensor values)
      /// This basically means we have 16 values increments.

      if ((gbl_cur_cmd == CMD_MISC_CONTROL) && (gbl_cur_param == MISC_UPLOAD_EEPROM)) {


         uploadLen = ((((int16)gbl_cur_ext<<8) + gbl_cur_ext_byte)<<5);  // number of bytes to upload

         #if defined(__PCM__)
         

         // if uploadlen is 0 -> use automatic upload. In this case upload len becomes the last EEPROM location
         // referenced by the 'record' command. This value is stored in the 'internal' EEPROM of
         // the PIC.
         if (uploadLen == 0) {
            uploadLen = read_ext_eeprom(MEM_PTR_LOG_BASE_ADDRESS) + ((int16)read_ext_eeprom(MEM_PTR_LOG_BASE_ADDRESS+1) << 8);
         }

         // send EEPROM upload header bytes + number of bytes to send
         printf( "%c%c%c%c", EEPROMuploadHeader1, EEPROMuploadHeader2, uploadLen & 0xff, uploadLen >> 8);

         for (counter = 0 ; counter < uploadLen ; counter++ )
            printf( "%c", read_ext_eeprom(RECORD_BASE_ADDRESS + counter));
      

         #elif defined(__PCH__)
      
         // if uploadlen is 0 -> use automatic upload. In this case upload len becomes the last EEPROM location
         // referenced by the 'record' command. This value is stored in the 'internal' EEPROM of
         // the PIC.
         if (uploadLen == 0) {
            uploadLen = read_program_eeprom(MEM_PTR_LOG_BASE_ADDRESS<<1);
         }

         // number of bytes to send to the PC is twice the number of words to upload
         uploadLen<<=1;
         
         // send EEPROM upload header bytes + number of bytes to send
         printf( "%c%c%c%c", EEPROMuploadHeader1, EEPROMuploadHeader2, uploadLen & 0xff, uploadLen >> 8);
         
         // shift back becuase the nubmer of words read is half the number of bytes
         uploadLen>>=1;
         
         for (counter = 0 ; counter < uploadLen ; counter++ ) {
            foo = read_program_eeprom((RECORD_BASE_ADDRESS + counter)<<1);
            printf( "%c%c", foo&0xff, foo>>8 );
         }
            
      
      #endif

      }

      CMD_STATE = WAITING_FOR_FIRST_HEADER;

   }


   else   // NO_CMD
   {
      DoSensorStuff();
   }

   // if Logo is running
  if (gblLogoIsRunning)
   {
            // if wait command is not in progress -> run next code
            if (!gblWaitCounter) {
               
               evalOpcode(fetchNextOpcode());
            }
   }

   ///////////////////////////////////////////////////////////////
   // make sure the CMD_STATE is not stuck somewhere for too long

   // gblCmdTimeOut is incremented in Timer1's ISR every 0.1 sec
   if (gblCmdTimeOut > CMD_TIMEOUT_PERIOD) {
      CMD_STATE = WAITING_FOR_FIRST_HEADER;
      gblCmdTimeOut = 0;
      
   }



}

}


#include <logovm.c>


